#include <pluginlib/class_list_macros.h>
#include <tf/LinearMath/Transform.h>
#include <tf/transform_datatypes.h>
#include "myplanner.h"
PLUGINLIB_EXPORT_CLASS(yyc_planner::MyGlobalPlanner, nav_core::BaseGlobalPlanner)
namespace yyc_planner{
MyGlobalPlanner::MyGlobalPlanner()
{

}
MyGlobalPlanner::MyGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{
    initialize(name, costmap_ros);
}

void MyGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{

}
bool MyGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, 
                const geometry_msgs::PoseStamped& goal,
                std::vector<geometry_msgs::PoseStamped>& plan)
                {
                    plan.push_back(start);
                    int N = 10;
                    double h = 1.0/N;
                    double dx = goal.pose.position.x - start.pose.position.x;
                    double dy = goal.pose.position.y - start.pose.position.y;
                    double goal_yaw  = tf::getYaw(goal.pose.orientation);
                    double start_yaw = tf::getYaw(start.pose.orientation);
                    for (int i=0; i<N; i++){
                        geometry_msgs::PoseStamped new_goal = goal;
                        double yaw = start_yaw + h*i*(goal_yaw - start_yaw);
                        tf::Quaternion goal_quat = tf::createQuaternionFromYaw(yaw);
                        new_goal.pose.position.x = start.pose.position.x + h*i*dx;
                        new_goal.pose.position.y = start.pose.position.y + h*i*dy;
                        new_goal.pose.orientation.x = goal_quat.x();
                        new_goal.pose.orientation.y = goal_quat.y();
                        new_goal.pose.orientation.z = goal_quat.z();
                        new_goal.pose.orientation.w = goal_quat.w();
                        plan.push_back(new_goal);
                    }
                    plan.push_back(goal);
                    return true;
                }
}